﻿// <copyright file="Quaternion.cs" company="Community Combat Simulator">
//    Community Combat Simulator: a free combat flight simulator.
//    Copyright (C) 2008 Aaron Schinkowitch
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see http://www.gnu.org/licenses/.
// </copyright>
namespace CCSim.Vecmath
{
    using System;
    using System.Globalization;
    using CCSim.Util;

    /// <summary>
    /// Quaternions provide a convenient mathematical notation for representing orientations and
    /// rotations of objects.
    /// </summary>
    public class Quaternion
    {
        /// <summary>
        /// The margin of allowed error when normalizing a quaternion.
        /// </summary>
        private const double NormalizationMargin = 1e-6;
        
        /// <summary>
        /// The quaternion's components.
        /// </summary>
        private double w, x, y, z;
        
        /// <summary>
        /// Initializes a new instance of the Quaternion class that is an identity quaternion.
        /// </summary>
        public Quaternion()
        {
            this.SetComponents(1.0, 0.0, 0.0, 0.0);
        }
        
        /// <summary>
        /// Initializes a new instance of the Quaternion class with the same component values as the
        /// specified quaternion.
        /// </summary>
        /// <param name="other">The quaternion whose component values are copied.</param>
        /// <exception cref="ArgumentNullException">Null quaternion.</exception>
        public Quaternion(Quaternion other)
        {
            if (other == null)
            {
                throw new ArgumentNullException("other");
            }
            
            this.SetComponents(other.W, other.X, other.Y, other.Z);
        }

        /// <summary>
        /// Initializes a new instance of the Quaternion class with the specified component values.
        /// </summary>
        /// <param name="w">The w component.</param>
        /// <param name="x">The x component.</param>
        /// <param name="y">The y component.</param>
        /// <param name="z">The z component.</param>
        internal Quaternion(double w, double x, double y, double z)
        {
            this.SetComponents(w, x, y, z);
        }
        
        /// <summary>
        /// Gets or sets the quaternion's w component.
        /// </summary>
        internal double W 
        {
            get 
            { 
                return this.w;
            }
            
            set
            {
                this.CheckComponentValue("W", value);
                this.w = value;
            }
        }
        
        /// <summary>
        /// Gets or sets the quaternion's x component.
        /// </summary>
        internal double X 
        {
            get 
            {
                return this.x; 
            }
            
            set
            {
                this.CheckComponentValue("X", value);
                this.x = value;
            }
        }
        
        /// <summary>
        /// Gets or sets the quaternion's y component.
        /// </summary>
        internal double Y 
        {
            get 
            {
                return this.y;
            }
            
            set
            {
                this.CheckComponentValue("Y", value);
                this.y = value;
            }
        }
        
        /// <summary>
        /// Gets or sets the quaternion's z component.
        /// </summary>
        internal double Z 
        {
            get 
            { 
                return this.z;
            }
            
            set
            {
                this.CheckComponentValue("Z", value);
                this.z = value;
            }
        }
        
        /// <summary>
        /// Gets the square of the length of the quaternion, where the length is defined as the 
        /// the square root of the sum of the squares of the components.
        /// </summary>
        private double LengthSquared
        {
            get
            {
                return (this.W * this.W) + (this.X * this.X) + (this.Y * this.Y) 
                    + (this.Z * this.Z);
            }
        }

        /// <summary>
        /// Performs vector / quaternion multiplication.
        /// </summary>
        /// <param name="left">The left hand operand.</param>
        /// <param name="right">The right hand operand.</param>
        /// <returns>A new quaternion containing the results the multiplication.</returns>
        public static Quaternion operator *(Vector left, Quaternion right)
        {
            if (left == null)
            {
                throw new ArgumentNullException("left", "Left vector cannot be null");
            }
            
            return new Quaternion(0, left.X, left.Y, left.Z).MultiplyBy(right);
        }
        
        /// <summary>
        /// Sets the components of the quaternion to those of the argument.
        /// </summary>
        /// <param name="quaternion">The quaternion whose value should be copied.</param>
        public void SetComponents(Quaternion quaternion)
        {
            if (quaternion == null)
            {
                throw new ArgumentNullException("quaternion");
            }
            
            this.SetComponents(quaternion.W, quaternion.X, quaternion.Y, quaternion.Z);
        }

        /// <summary>
        /// Calculates a hash code for the quaterion.
        /// </summary>
        /// <returns>The hash code.</returns>
        public override int GetHashCode()
        {
            return this.W.GetHashCode();
        }
        
        /// <summary>
        /// Checks for equality. Two quaternions are equal if their components are equal.
        /// </summary>
        /// <param name="obj">The object to check.</param>
        /// <returns>true if the argument is equal to the quaternion, false otherwise.</returns>
        public override bool Equals(object obj)
        {
            Quaternion other = obj as Quaternion;
            
            Equality equality = new Equality();
            
            return other != null 
                && equality.RoughlyEqual(this.W, other.W)
                && equality.RoughlyEqual(this.X, other.X)
                && equality.RoughlyEqual(this.Y, other.Y)
                && equality.RoughlyEqual(this.Z, other.Z);
        }

        /// <summary>
        /// Gets a string representation of the quaternion.
        /// </summary>
        /// <returns>A string representation of the quaternion.</returns>
        public override string ToString()
        {
            return String.Format(
                CultureInfo.InvariantCulture, 
                "{0} (w: {1} x: {2} y: {3} z: {4})", 
                this.GetType().Name, 
                this.W, 
                this.X,
                this.Y, 
                this.Z);
        }
        
        /// <summary>
        /// Normalizes the quaternion inline. After normalizing the quaternion, the sum of the
        /// squares of the component values will be 1.
        /// </summary>
        /// <returns>The quaternion itself.</returns>
        public Quaternion Normalize()
        {
            double lengthSquared = this.LengthSquared;
            
            if (new Equality().RoughlyEqual(lengthSquared, 1.0, NormalizationMargin))
            {
                return this;
            }
            
            if (lengthSquared < NormalizationMargin)
            {
                this.SetComponents(1, 0, 0, 0);
                return this;
            }
            
            return this.Scale(1.0 / Math.Sqrt(lengthSquared));
        }
        
        /// <summary>
        /// Checks if the quaternion is normalized.
        /// </summary>
        /// <returns>true if the quaternion is normalized, false otherwise.</returns>
        public bool IsNormalized()
        {
            return new Equality().RoughlyEqual(this.LengthSquared, 1.0, NormalizationMargin);
        }
        
        /// <summary>
        /// Scales the components of the quaternion by the specified scalar.
        /// </summary>
        /// <param name="scalar">The scalar.</param>
        /// <returns>The quaternion itself.</returns>
        public Quaternion Scale(double scalar)
        {
            this.W *= scalar;
            this.X *= scalar;
            this.Y *= scalar;
            this.Z *= scalar;
            
            return this;
        }
        
        /// <summary>
        /// Mulitplies the quaternion by the passed value inline such that 
        /// <para/>
        /// this = this * right.
        /// </summary>
        /// <param name="right">The right operand.</param>
        /// <returns>The quaternion itself.</returns>
        public Quaternion MultiplyBy(Quaternion right) 
        {
            if (right == null) 
            {
                throw new ArgumentNullException("right");
            }
            
            /*
                Q_w = A_w B_w - A_x B_x - A_y B_y - A_z B_z
                Q_x = A_w B_x + A_x B_w + A_y B_z - A_z B_y
                Q_y = A_w B_y - A_x B_z + A_y B_w + A_z B_x
                Q_z = A_w B_z + A_x B_y - A_y B_x + A_z B_w
             */
            
            this.SetComponents(
                (this.W * right.W) - (this.X * right.X) - (this.Y * right.Y) - (this.Z * right.Z),
                (this.W * right.X) + (this.X * right.W) + (this.Y * right.Z) - (this.Z * right.Y),
                (this.W * right.Y) - (this.X * right.Z) + (this.Y * right.W) + (this.Z * right.X),
                (this.W * right.Z) + (this.X * right.Y) - (this.Y * right.X) + (this.Z * right.W));
            
            return this;
        }
        
        /// <summary>
        /// Rotate inline a vector by the rotation specified in the quaternion.
        /// </summary>
        /// <param name="vector">The vector to rotate.</param>
        public void Rotate(Vector vector)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }
            
            // t1 = R * t * R*
            Quaternion temp = new Quaternion(this);
            Quaternion vectorQuaternion = new Quaternion(0, vector.X, vector.Y, vector.Z);
            Quaternion conjugate = new Quaternion(this.W, -this.X, -this.Y, -this.Z);
            
            temp.MultiplyBy(vectorQuaternion).MultiplyBy(conjugate);
            
            vector.X = temp.X;
            vector.Y = temp.Y;
            vector.Z = temp.Z;
        }
        
        /// <summary>
        /// Sets the components of the quaternion to the specified values.
        /// </summary>
        /// <param name="w">The w component.</param>
        /// <param name="x">The x component.</param>
        /// <param name="y">The y component.</param>
        /// <param name="z">The z component.</param>
        internal void SetComponents(double w, double x, double y, double z)
        {
            this.W = w;
            this.X = x;
            this.Y = y;
            this.Z = z;
        }

        /// <summary>
        /// Validates the value that will set on a component.
        /// </summary>
        /// <param name="component">The component being validated.</param>
        /// <param name="value">The value to check.</param>
        private void CheckComponentValue(string component, double value)
        {
            if (Double.IsNaN(value) || Double.IsInfinity(value))
            {
                throw new ArgumentOutOfRangeException(
                    "value", "Cannot set " + component + " to " + value);
            }
        }
    }
}
